#!/home/st/ubuntu_data/software/miniconda3/envs/pointrcnn_ros/bin/python
import apriltag
import cv2
import numpy as np
import rospy
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

at_detector = apriltag.Detector(apriltag.DetectorOptions(families='tag36h11 tag25h9') )
cameraParams_Intrinsic = [384.1054992675781, 320.1566147348167, 384.8056640625,237.8698444374022]  # camera_fx, camera_fy, camera_cx, camera_cy
camera_matrix = np.array(([384.1054992675781, 0,320.1566147348167],
                         [0,  384.8056640625, 237.8698444374022],
                         [0, 0, 1.0]), dtype=np.double) 

def callback(data):
    # define picture to_down' coefficient of ratio
    scaling_factor = 0.5
    global count,bridge
    count = count + 1
    if count == 1:
        count = 0
        cv_img = bridge.imgmsg_to_cv2(data, "bgr8")
        gray = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
        tags = at_detector.detect(gray)

        for tag in tags:
            cv2.circle(cv_img, tuple(tag.corners[0].astype(int)), 4, (255, 0, 0), 2) # left-top
            cv2.circle(cv_img, tuple(tag.corners[1].astype(int)), 4, (255, 0, 0), 2) # right-top
            cv2.circle(cv_img, tuple(tag.corners[2].astype(int)), 4, (255, 0, 0), 2) # right-bottom
            cv2.circle(cv_img, tuple(tag.corners[3].astype(int)), 4, (255, 0, 0), 2) # left-bottom
            M, init_error, final_error = at_detector.detection_pose(tag,
                                        camera_params=cameraParams_Intrinsic,
                                        tag_size=0.4
                                              )
            print("x y z ",M[0,3], "  ", M[1,3],"  ", M[2,3])


        cv2.imshow("frame" , cv_img)
        cv2.waitKey(3)
    else:
        pass
 
def displayWebcam():
    rospy.init_node('webcam_display', anonymous=True)
 
    # make a video_object and init the video object
    global count,bridge
    count = 0
    bridge = CvBridge()
    rospy.Subscriber('/camera/color/image_raw', Image, callback)
    rospy.spin()
 
if __name__ == '__main__':
    displayWebcam()
